#include "pcpt_comm/pcpt_comm_ros.h"

/**************************************************************************************
功能描述: sensor_msgs::PointCloud2 转 pcl::PCLPointCloud2
修改记录:
**************************************************************************************/
void POINT_CLOUD_MSG_TRANSFORM_C::MsgRosToMsgPcl(const sensor_msgs::PointCloud2 &inPut, pcl::PCLPointCloud2 &outPut)
{
    pcl_conversions::toPCL(inPut, outPut);
}

/**************************************************************************************
功能描述:sensor_msgs::PointCloud2 转 pcl::PointCloud< PointT>
修改记录:
**************************************************************************************/
void POINT_CLOUD_MSG_TRANSFORM_C::MsgRosToMsgPcl(const sensor_msgs::PointCloud2 &inPut, pcl::PointCloud<MyPoint> &outPut)
{
    pcl::fromROSMsg(inPut, outPut);
}

/**************************************************************************************
功能描述: pcl::PCLPointCloud2 转 sensor_msgs::PointCloud2
修改记录:
**************************************************************************************/
void POINT_CLOUD_MSG_TRANSFORM_C::MsgPclToMgRos(const pcl::PCLPointCloud2& inPut, sensor_msgs::PointCloud2 &outPut)
{
    pcl_conversions::fromPCL(inPut, outPut);
}

/**************************************************************************************
功能描述: pcl::PCLPointCloud2 转 pcl::PointCloud< PointT>
修改记录:
**************************************************************************************/
void POINT_CLOUD_MSG_TRANSFORM_C::MsgPclToMsgPcl(const pcl::PCLPointCloud2 &inPut, pcl::PointCloud<MyPoint> &outPut)
{
    pcl::fromPCLPointCloud2(inPut, outPut);
}

/**************************************************************************************
功能描述: pcl::PointCloud<PointT> 转 snesor_msgs::PointCloud2
修改记录:
**************************************************************************************/
void POINT_CLOUD_MSG_TRANSFORM_C::MsgPclToMsgRos(const pcl::PointCloud<MyPoint> &inPut, sensor_msgs::PointCloud2 &outPut)
{
    pcl::toROSMsg(inPut, outPut);
}

/**************************************************************************************
功能描述: pcl::PointCloud< PointT> 转 pcl::PCLPointCloud2
修改记录:
**************************************************************************************/
void POINT_CLOUD_MSG_TRANSFORM_C::MsgPclToMsgPcl(const pcl::PointCloud<MyPoint> &inPut, pcl::PCLPointCloud2 &outPut)
{
    pcl::toPCLPointCloud2(inPut, outPut);
}

/**************************************************************************************
功能描述: sensor_msgs::PointCloud -> sensor_msgs::PointCloud2 
修改记录:
**************************************************************************************/
void POINT_CLOUD_MSG_TRANSFORM_C::ConvertPointCloudToPointCloud2(const sensor_msgs::PointCloud &inPut, sensor_msgs::PointCloud2 &outPut)
{
    sensor_msgs::convertPointCloudToPointCloud2(inPut, outPut);
}

/**************************************************************************************
功能描述: sensor_msgs::PointCloud2 -> sensor_msgs::PointCloud
修改记录:
**************************************************************************************/
void POINT_CLOUD_MSG_TRANSFORM_C::ConvertPointCloud2ToPointCloud(const sensor_msgs::PointCloud2 &inPut, sensor_msgs::PointCloud &outPut)
{
    sensor_msgs::convertPointCloud2ToPointCloud(inPut, outPut);
}

/**************************************************************************************
功能描述: 点云发布
修改记录:
**************************************************************************************/
void POINT_CLOUD_MSG_SENDER_C::Send(sensor_msgs::PointCloud2 &cloud) const
{
    if (publisher_.getNumSubscribers()) {
        cloud.header.stamp = ros::Time::now();
        cloud.header.frame_id = "base_link";
        publisher_.publish(cloud);
    }
}

/**************************************************************************************
功能描述: 
修改记录:
**************************************************************************************/
BOOL POINT_CLOUD_MSG_SENDER_C::HasSubscriber() const
{
    if (publisher_.getNumSubscribers())
        return true;

    return false;
}

/**************************************************************************************
功能描述: 2D坐标数组发布
修改记录:
**************************************************************************************/
void COOR_2D_ARRAY_MSG_SENDER_C::Send(comm_msg::coor2DArray &coor2DArray)
{
    if (publisher_.getNumSubscribers())
        publisher_.publish(coor2DArray);
}

/**************************************************************************************
功能描述: 
修改记录:
**************************************************************************************/
void FUSED_STATIC_OBST_SENDER_C::Send(const list<COORDINATE_2D_STRU> &lstObstByMap, PLAN_MOVE_TREND_ENUM enPlanMoveTrend)
{
    comm_msg::coor2D      coor2D;
    comm_msg::coor2DArray coor2DArray;
    switch(enPlanMoveTrend) {
        case PLAN_MOVE_TREND_FORWARD:
            coor2DArray.isContainForwardObst  = 1;
            coor2DArray.isContainBackwardObst = 0;
            break;
        case PLAN_MOVE_TREND_BACKWARD:
            coor2DArray.isContainForwardObst  = 0;
            coor2DArray.isContainBackwardObst = 1;
            break;
        case PLAN_MOVE_TREND_FREE:
            coor2DArray.isContainForwardObst  = 1;
            coor2DArray.isContainBackwardObst = 1;
            break;
        case PLAN_MOVE_TREND_STOP:
            coor2DArray.isContainForwardObst  = 0;
            coor2DArray.isContainBackwardObst = 0;
            break;
        default:
            coor2DArray.isContainForwardObst  = 0;
            coor2DArray.isContainBackwardObst = 0;
            break;
    }

    pcl::PointCloud<pcl::PointXYZ>::Ptr pCloudByMap(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr pCloudByCar(new pcl::PointCloud<pcl::PointXYZ>);
    list<COORDINATE_2D_STRU>::const_iterator iter;
    for (iter = lstObstByMap.begin(); iter != lstObstByMap.end(); ++iter) {
        coor2D.pos_x = iter->x;
        coor2D.pos_y = iter->y;
        coor2DArray.coor2DArray.push_back(coor2D);

        COORDINATE_2D_STRU stCarPosByCar;
        objCar_.TransformPosByMapToByCar(*iter, stCarPosByCar);

        pcl::PointXYZ point;
        
        point.z = 0;

        point.x = iter->x;
        point.y = iter->y;
        pCloudByMap->points.push_back(point);
        
        point.x = stCarPosByCar.x;
        point.y = stCarPosByCar.y;
        pCloudByCar->points.push_back(point);
    }

    objCoor2DArrayMsgSender_.Send(coor2DArray);
    objPointCloudMsgSenderByMap_.Send(pCloudByMap, "map");
    objPointCloudMsgSenderByCar_.Send(pCloudByCar, "base_link");
}

/**************************************************************************************
功能描述: 发布移动目标跟踪预测Marker
修改记录:
**************************************************************************************/
void FUSED_DYNAMIC_OBST_SENDER_C::Send(const comm_msg::movTargetsArray &msgMovTargetsArray)
{
    if (pubMovTargets_.getNumSubscribers())
        pubMovTargets_.publish(msgMovTargetsArray);

    if (pubMarker_.getNumSubscribers() == 0)
        return;
    
    vector<comm_msg::movTarget>::const_iterator iter_i;
    vector<comm_msg::movPred>::const_iterator iter_j;

    for (iter_i = msgMovTargetsArray.movTargetsArray.begin(); iter_i != msgMovTargetsArray.movTargetsArray.end(); ++iter_i) {
        visualization_msgs::Marker points, line_list;
    	geometry_msgs::Point pos_begin, pos_end, pos_last;
    
        UINT32 index = iter_i - msgMovTargetsArray.movTargetsArray.begin();
        float  size = msgMovTargetsArray.movTargetsArray.size() + 1.0;
      
      	points.header       		  	= msgMovTargetsArray.header;
    	points.ns						= "track_pred";
    	points.action					= visualization_msgs::Marker::ADD;
    	points.pose.orientation.w	 	= 1.0;
    	points.id						= index + 1;
    	points.type 					= visualization_msgs::Marker::POINTS;
    	points.scale.x					= 0.2;
    	points.scale.y					= 0.2;
    	points.color.r					= 1.0f;     //0.0~1.0
        points.color.g					= index / size;
        points.color.b					= 1- index / size;
    	points.color.a					= 1.0;

    	line_list.header		        = msgMovTargetsArray.header;
    	line_list.ns					= "track_pred";
    	line_list.action				= visualization_msgs::Marker::ADD;
    	line_list.pose.orientation.w	= 1.0;
    	line_list.id					= index + 201;
    	line_list.type					= visualization_msgs::Marker::LINE_LIST;
    	line_list.scale.x				= 0.1;
    	line_list.color.r				= 1.0;
    	line_list.color.g				= index / size;
    	line_list.color.b				= 1 - index / size;
    	line_list.color.a				= 1.0;
        
        for (iter_j = iter_i->movPredArray.begin(); iter_j != iter_i->movPredArray.end(); ++iter_j) {
            pos_begin.x = iter_j->coor2D.pos_x;
            pos_begin.y = iter_j->coor2D.pos_y;
            points.points.push_back(pos_begin);

            if (iter_j == iter_i->movPredArray.begin()) {
                pos_end.x = iter_i->pos_x;
                pos_end.y = iter_i->pos_y;
            } else {
                pos_end = pos_last;
            }
            
            line_list.points.push_back(pos_begin);
    	    line_list.points.push_back(pos_end);

            pos_last = pos_begin;
        }

        pubMarker_.publish(points);
	    pubMarker_.publish(line_list);
	}
}

/**************************************************************************************
功能描述: 判断是否异常
修改记录:
**************************************************************************************/
BOOL LIDAR_MSG_RECEIVER_C::IsDataInvalid() const
{
    return objDataInvalidTimer_.IsTimeOut();
}

/**************************************************************************************
功能描述: 判断是否故障
修改记录:
**************************************************************************************/
BOOL LIDAR_MSG_RECEIVER_C::IsSensorFault() const
{
    return objSensorFaultTimer_.IsTimeOut();
}

/**************************************************************************************
功能描述: 获取点云
修改记录:
**************************************************************************************/
void LIDAR_MSG_RECEIVER_C::GetNormalPointsCloud(MyCloud::Ptr &ptrCloud) const
{
    if (objDataInvalidTimer_.IsTimeOut()) {
        ptrCloud->clear();
        return;
    }

    GetPointsCloud(ptrCloud);
}

/**************************************************************************************
功能描述: 雷达点云接收
修改记录:
**************************************************************************************/
void LIDAR_MSG_RECEIVER_C::MsgReceiveCallBack(const sensor_msgs::PointCloud2ConstPtr &pstMsg)
{
    if (pstMsg->data.size()) {
        pcl::fromROSMsg(*pstMsg, *ptrCloud_);
        TimerReset();
    }
}

/**************************************************************************************
功能描述: 获取点云
修改记录:
**************************************************************************************/
void LIDAR_MSG_RECEIVER_C::GetPointsCloud(MyCloud::Ptr &ptrCloud) const
{
    *ptrCloud = *ptrCloud_;
}

/**************************************************************************************
功能描述: 定时器复位
修改记录:
**************************************************************************************/
void LIDAR_MSG_RECEIVER_C::TimerReset()
{
    objDataInvalidTimer_.Reset();
    objSensorFaultTimer_.Reset();
}

/**************************************************************************************
功能描述: 判断是否异常
修改记录:
**************************************************************************************/
BOOL RADAR_MSG_RECEIVER_C::IsDataInvalid() const
{
    return objDataInvalidTimer_.IsTimeOut();
}

/**************************************************************************************
功能描述: 判断是否故障
修改记录:
**************************************************************************************/
BOOL RADAR_MSG_RECEIVER_C::IsSensorFault() const
{
    return objSensorFaultTimer_.IsTimeOut();
}

/**************************************************************************************
功能描述: 获取毫米波雷达最新消息内容
修改记录:
**************************************************************************************/
const RADAR_INFO_STRU *RADAR_MSG_RECEIVER_C::Get(void)
{
    if (objDataInvalidTimer_.IsTimeOut()) {
        return NULL;
    }
    
    return &stRadarInfo_;
}

/**************************************************************************************
功能描述:毫米波雷达信息
修改记录:
**************************************************************************************/
void RADAR_MSG_RECEIVER_C::ReceiveMsgCallBack(const comm_msg::radarTargetArray::ConstPtr &pstMsg)
{
    TimerReset();
    
    stRadarInfo_.radarID = pstMsg->radarID;
    stRadarInfo_.timeStamp = pstMsg->header.stamp.toSec();
    stRadarInfo_.vstPosVel.clear();

    vector<comm_msg::radarTarget>::const_iterator iter;
    POS_VEL_STRU stPosVel;
    for (iter = pstMsg->allTargets.begin(); iter != pstMsg->allTargets.end(); ++iter) {
        stPosVel.posX = iter->targetDistx;
        stPosVel.posY = iter->targetDisty;
        stPosVel.velX  = iter->targetVelx ;
        stPosVel.velY  = iter->targetVely ;
        stRadarInfo_.vstPosVel.push_back(stPosVel);
    }
}

/**************************************************************************************
功能描述: 定时器复位
修改记录:
**************************************************************************************/
void RADAR_MSG_RECEIVER_C::TimerReset()
{
    objDataInvalidTimer_.Reset();
    objSensorFaultTimer_.Reset();
}

/**************************************************************************************
功能描述: 深度相机数据接收
修改记录:
**************************************************************************************/
void DEPTH_CAMERA_MSG_RECEIVER_C::MsgReceiveCallBack(const sensor_msgs::PointCloud2ConstPtr &pstMsg)
{
    if (pstMsg->data.size()) {
        pcl::fromROSMsg(*pstMsg, *ptrCloud_);
        TimerReset();
    }
}

/**************************************************************************************
功能描述: 获取相机深度点云数据
修改记录:
**************************************************************************************/
void DEPTH_CAMERA_MSG_RECEIVER_C::GetPointsCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr &ptrCloud) const
{
    *ptrCloud = *ptrCloud_;
}

/**************************************************************************************
功能描述: 判断是否超时
修改记录:
**************************************************************************************/
BOOL DEPTH_CAMERA_MSG_RECEIVER_C::IsDataInvalid() const
{
    return objDataInvalidTimer_.IsTimeOut();
}

/**************************************************************************************
功能描述: 状态复位 ？todo
修改记录:
**************************************************************************************/
BOOL DEPTH_CAMERA_MSG_RECEIVER_C::IsSensorReset() const
{
    return objSensorResetTimer_.IsTimeOut();
}

/**************************************************************************************
功能描述: 是否有故障
修改记录:
**************************************************************************************/
BOOL DEPTH_CAMERA_MSG_RECEIVER_C::IsSensorFault() const
{
    return objSensorFaultTimer_.IsTimeOut();
}

/**************************************************************************************
功能描述: 复位
修改记录:
**************************************************************************************/
void DEPTH_CAMERA_MSG_RECEIVER_C::TimerReset()
{
    objDataInvalidTimer_.Reset();
    objSensorResetTimer_.Reset();
    objSensorFaultTimer_.Reset();
}

/**************************************************************************************
功能描述: 获取普通点云
修改记录:
**************************************************************************************/
void DEPTH_CAMERA_MSG_RECEIVER_C::GetNormalPointsCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr &ptrCloud) const
{
    if (objDataInvalidTimer_.IsTimeOut()) {
        ptrCloud->clear();
        return;
    }

    GetPointsCloud(ptrCloud);
}

/**************************************************************************************
功能描述: 选择深度相机enum
修改记录:
**************************************************************************************/
void SELECT_DEPTH_CAMERA_MSG_RECEIVER_C::ReceiveMsgCallBack(const comm_msg::visionDepthCamera::ConstPtr &pstMsg)
{   
    switch (pstMsg->selectDepthCamera) {
        case 0:
            enSelectDepthCamera_ = DEPTH_CAMERA_NULL;
            break;
            
        case 1:
            enSelectDepthCamera_ = DEPTH_CAMERA_FRONT;
            break;

        case 2:
            enSelectDepthCamera_ = DEPTH_CAMERA_BACK;
            break;

        case 3:
            enSelectDepthCamera_ = DEPTH_CAMERA_LEFT;
            break;

        case 4:            
            enSelectDepthCamera_ = DEPTH_CAMERA_RIGHT;
            break;

        default:
            enSelectDepthCamera_ = DEPTH_CAMERA_NULL;
            break;
    }
    
    ST_LOG_INFO("Receive Select Depth Camera: %d.", pstMsg->selectDepthCamera);
}

/**************************************************************************************
功能描述: 获取选择深度相机enum
修改记录:
**************************************************************************************/
void SELECT_DEPTH_CAMERA_MSG_RECEIVER_C::Get(DEPTH_CAMERA_ENUM &enSelectDepthCamera)
{
    enSelectDepthCamera = enSelectDepthCamera_;
}

/**************************************************************************************
功能描述: 发布深度相机点云
修改记录:
**************************************************************************************/
void CAMERA_PCD_SENDER_C::Send(pcl::PointCloud<pcl::PointXYZ>::Ptr pCloud)
{
    sensor_msgs::PointCloud2 msg;
    
    pcl::toROSMsg(*pCloud, msg);
    msg.header.frame_id = "/base_link";
    msg.header.stamp    = ros::Time::now();

    if (0 != publisher_.getNumSubscribers()) {
        publisher_.publish(msg);
    }
}

/**************************************************************************************
功能描述: 发布与视觉模块交互状态
修改记录:
**************************************************************************************/
void VISION_STATE_SENDER_C::Send(UINT08 visionState)
{
    comm_msg::visionState msg;
    if (0 != publisher_.getNumSubscribers()) {
        msg.visionState = visionState;
        publisher_.publish(msg);
    }
}

